#include "Kalman.h"

KalmanFilter::KalmanFilter()
{

}

KalmanFilter::~KalmanFilter()
{

}

bool KalmanFilter::isInitialized()
{
    return is_initializated_;
}

void KalmanFilter::Initialization(Eigen::VectorXd &state_in)
{
    m_state = state_in;
    is_initializated_ = true;
}

void KalmanFilter::Prediction()
{
    m_state = m_F * m_state;
    static Eigen::MatrixXd Ft = m_F.transpose();
    m_P = m_F * m_P * Ft + m_Q_;
}

void KalmanFilter::MeasurementUpdate(const Eigen::VectorXd &z)
{
    Eigen::VectorXd y = z - m_H * m_state;
    Eigen::MatrixXd S = m_H * m_P * m_H.transpose() + m_R_;
    Eigen::MatrixXd K = m_P * m_H.transpose() * S.inverse();
    m_state = m_state + (K*y);
    int size = m_state.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size,size);
    m_P = (I - K*m_H)*m_P;
}

int KalmanFilter::getState(Eigen::VectorXd &out)
{
    out = m_state;
    return 0;
}

void KalmanFilter::setF(const Eigen::MatrixXd &F)
{
    m_F = F;
}

void KalmanFilter::setH(const Eigen::MatrixXd &H)
{
    m_H = H;
}

void KalmanFilter::setP(const Eigen::MatrixXd &P)
{
    m_P = P;
}

void KalmanFilter::setQ(const Eigen::MatrixXd &Q)
{
    m_Q_ = Q;
}

void KalmanFilter::setR(const Eigen::MatrixXd &R)
{
    m_R_ = R;
}
